#include "Step_moter.h"
#ifdef CPU_SELECT_AT32F415XX
#include "at32f415.h"
#endif
extern StepMotor StepMotor_1 ;
extern StepMotor StepMotor_2 ;
extern StepMotor StepMotor_3 ;
extern StepMotor StepMotor_4 ;

#define STEP_MOTOR_MODE_MASK      0x00000001
#define STEP_MOTOR_MODE_POSITION  0x00000001
#define STEP_MOTOR_MODE_VELOCITY  0x00000000

#define STEP_MOTOR_DIR_MASK       0x00000002
#define STEP_MOTOR_DIR_FORWARD    0x00000002
#define STEP_MOTOR_DIR_BACKWARD   0x00000000

#define STEP_MOTOR_STATE_MASK     0x00000004
#define STEP_MOTOR_STATE_RUN      0x00000004
#define STEP_MOTOR_STATE_STOP     0x00000000

#define STEP_MOTOR_ARRIVED_TARGET_STEP_MASK  0x00000008
#define STEP_MOTOR_ARRIVED_TARGET_STEP_TURE  0x00000008
#define STEP_MOTOR_ARRIVED_TARGET_STEP_FAULT 0x00000000


#define STEP_MOTOR_ARRIVED_TARGET_VECL_MASK  0x00000010
#define STEP_MOTOR_ARRIVED_TARGET_VECL_TURE  0x00000010
#define STEP_MOTOR_ARRIVED_TARGET_VECL_FAULT 0x00000000

#define STEP_MOTOR_PREPARE_STOP_MASK         0x00000020
#define STEP_MOTOR_PREPARE_STOP_TURE         0x00000020
#define STEP_MOTOR_PREPARE_STOP_FAULT        0x00000020

#define STEP_MOTOR_VELOCITY_CONTROL_MASK     (STEP_MOTOR_PREPARE_STOP_MASK | STEP_MOTOR_ARRIVED_TARGET_VECL_MASK)

#ifdef CPU_SELECT_AT32F415XX
// #define PWM_START_IT(motor, channel) tmr_interrupt_enable(motor->TIM,TMR_C1_INT,TRUE)
// #define PWM_START(motor,channel)     tmr_channel_enable(motor->TIM,motor->Channel)
// #define PWM_STOP_IT(motor, channel) (motor->TIM->iden_bit.c##channel##ien = 0)
// #define PWM_STOP(motor,channel)     (motor->TIM->cctrl_bit.c##channel##en = 0)
void PWM_START_IT(StepMotor *motor)
{
  uint32_t interrupt_type = TMR_C1_INT;
  if(motor.Channel == TMR_SELECT_CHANNEL_2)
  {
    interrupt_type = TMR_C2_INT;
  }
  else if(motor.Channel == TMR_SELECT_CHANNEL_3)
  {
    interrupt_type = TMR_C3_INT;
  }
  else if(motor.Channel == TMR_SELECT_CHANNEL_4)
  {
    interrupt_type = TMR_C4_INT;
  }

  tmr_interrupt_enable(motor->TIM,interrupt_type,TRUE);
}
void PWM_STOP_IT(StepMotor *motor)
{
  uint32_t interrupt_type = TMR_C1_INT;
  if(motor->Channel == TMR_SELECT_CHANNEL_2)
  {
    interrupt_type = TMR_C2_INT;
  }
  else if(motor->Channel == TMR_SELECT_CHANNEL_3)
  {
    interrupt_type = TMR_C3_INT;
  }
  else if(motor->Channel == TMR_SELECT_CHANNEL_4)
  {
    interrupt_type = TMR_C4_INT;
  }

  tmr_interrupt_enable(motor->TIM,interrupt_type,FALSE);
}
void PWM_Start(StepMotor * motor)
{
  if(motor->Channel == TMR_SELECT_CHANNEL_1)
  {
    motor->TIM->cctrl_bit.c1en = TRUE;
  }
  else if(motor->Channel == TMR_SELECT_CHANNEL_2)
  {
    motor->TIM->cctrl_bit.c2en = TRUE;
  }
  else if(motor->Channel == TMR_SELECT_CHANNEL_3)
  {
    motor->TIM->cctrl_bit.c3en = TRUE;
  }
  else if(motor->Channel == TMR_SELECT_CHANNEL_4)
  {
    motor->TIM->cctrl_bit.c4en = TRUE;
  }
  
}
void PWM_Stop(StepMotor * motor)
{
  if(motor->Channel == TMR_SELECT_CHANNEL_1)
  {
    motor->TIM->cctrl_bit.c1en = FALSE;
  }
  else if(motor->Channel == TMR_SELECT_CHANNEL_2)
  {
    motor->TIM->cctrl_bit.c2en = FALSE;
  }
  else if(motor->Channel == TMR_SELECT_CHANNEL_3)
  {
    motor->TIM->cctrl_bit.c3en = FALSE;
  }
  else if(motor->Channel == TMR_SELECT_CHANNEL_4)
  {
    motor->TIM->cctrl_bit.c4en = FALSE;
  }
  
}
#endif

inline static void MotorRun(StepMotor *motor)
{
  motor->TIM->Instance->CNT  = 0;
  motor->TIM->Instance->ARR = 65535;
  motor->TIM->Instance->CCR1 = 65535 >> 1;
  motor->TIM->Instance->CNT  = 0;
  STEP_MOTOR_FLAGS(motor) |= STEP_MOTOR_STATE_RUN;
  PWM_Start_IT(motor); //hardware
  PWM_Start(motor); //hardware
}

inline static void MotorStop(StepMotor *motor)
{
  motor->TIM->Instance->ARR = 65535;
  motor->TIM->Instance->CCR1 = 65535 >> 1;
  motor->TIM->Instance->CNT  = 0;
  motor->Target_MicroStepPerSencond = 0;
  motor->Target_Step = 0;
  STEP_MOTOR_FLAGS(motor) = 0;
  PWM_Stop_IT(motor); //hardware
  PWM_Stop(motor); //hardware
}

int sqrt_bv(int n)
{
    int sqrt = 0;
    int shift = 15;
    int sqrt2;
    while (shift >= 0)
    {
        sqrt2 = ((sqrt << 1) + (1 << shift)) << shift;
        if (sqrt2 <= n)
        {
            sqrt += (1 << shift);
            n -= sqrt2;
        }
        shift--;
    }
    return sqrt;
}

__attribute__((used))  void StepMotorHandler(StepMotor *step_motor)
{
#define STEP(motor) (motor->Step)
#define CHECK_STEP(motor) (motor->Check_Setp)
#define FLAGS(motor) (motor->flags)
#define FREQ_PRE(motor) (motor->pre_f)
#define TARGET_VELOCITY(motor) (motor->Target_MicroStepPerSencond)
#define TARGET_POSITION(motor) (motor->Target_Step)
#define MIN_VELOCITY1 321
#define MIN_VELOCITY2 150
#define ACCE(motor) (motor->acce)
#define ALLOW_RANGE 6

#define TIM_ARR(motor) (motor->TIM->Instance->ARR) //hardware
#define TIM_CCR(motor) (motor->TIM->Instance->CCR1) //hardware
#define TIM_PSC 280 //hardware
#define TIM_CNT(motor) (motor->TIM->Instance->CNT) //hardware
#define TIM_CONVERT_K (step_motor->TIM_CNT_FREQ) //hardware
    int tmp = TIM_ARR(step_motor),tmp2 = FREQ_PRE(step_motor); //hardware
    ++STEP(step_motor);
    if((STEP(step_motor) >= CHECK_STEP(step_motor)))
    {
        if((FREQ_PRE(step_motor) < (TARGET_VELOCITY(step_motor) - ALLOW_RANGE)))
        {
            tmp2 = ((FREQ_PRE(step_motor) + sqrt_bv(FREQ_PRE(step_motor) * FREQ_PRE(step_motor) + ACCE(step_motor)))>>1);
            tmp = TIM_CONVERT_K / tmp2;
        }
        else if ((FREQ_PRE(step_motor) > TARGET_VELOCITY(step_motor) + ALLOW_RANGE))
        {
            tmp2 = FREQ_PRE(step_motor) * FREQ_PRE(step_motor) - ACCE(step_motor);
            if(tmp2 > 0)
            {
                tmp2 = (FREQ_PRE(step_motor) + sqrt_bv(tmp2))>>1;
                tmp = TIM_CONVERT_K / ((tmp2)); //ARR //hardware
            }
            else
            {
                tmp = 65535;
                tmp2 = TIM_CONVERT_K / tmp; //hardware
            }
        }
        else
        {
            FLAGS(step_motor) |= STEP_MOTOR_ARRIVED_TARGET_VECL_TURE;
        }

        if(tmp > 63335)
        {
            tmp = 65535;
        }
        TIM_ARR(step_motor) = tmp; //hardware
        TIM_CCR(step_motor) = tmp >> 1; //hardware
        FREQ_PRE(step_motor) = tmp2;
        CHECK_STEP(step_motor) += 1;
    }

    if((FLAGS(step_motor) & STEP_MOTOR_MODE_MASK) == STEP_MOTOR_MODE_POSITION)
    {
        if((STEP(step_motor) == (TARGET_POSITION(step_motor)>>1)) && ((FLAGS(step_motor) & STEP_MOTOR_PREPARE_STOP_MASK) == 0U))
        {
            TARGET_VELOCITY(step_motor) = MIN_VELOCITY1;
            FLAGS(step_motor) |= STEP_MOTOR_PREPARE_STOP_TURE;
        }
        else if(((FLAGS(step_motor) & STEP_MOTOR_VELOCITY_CONTROL_MASK)) == STEP_MOTOR_ARRIVED_TARGET_VECL_TURE)
        {
            TARGET_VELOCITY(step_motor) = MIN_VELOCITY1;
            CHECK_STEP(step_motor) = TARGET_POSITION(step_motor) - STEP(step_motor);
            FLAGS(step_motor) |= STEP_MOTOR_PREPARE_STOP_TURE;
        }
        else if(STEP(step_motor) == TARGET_POSITION(step_motor))
        {
            MotorStop(step_motor);
            FREQ_PRE(step_motor) = 0;
            FLAGS(step_motor) = 0;
        }
    }
    else
    {
        if((FLAGS(step_motor)&STEP_MOTOR_VELOCITY_CONTROL_MASK) == (STEP_MOTOR_PREPARE_STOP_TURE | STEP_MOTOR_ARRIVED_TARGET_VECL_TURE))
        {
            MotorStop(step_motor);
            FREQ_PRE(step_motor) = 0;
            FLAGS(step_motor) = 0;
        }
    }

#undef STEP
#undef CHECK_STEP
#undef FLAGS
#undef FREQ_PRE
#undef TARGET_VELOCITY
#undef TARGET_POSITION
#undef ACCE
#undef TIM_ARR
#undef TIM_PSC
#undef TIM_CONVERT_K
}

#define STEP_MOTOR_DIR_CONTROL_CODE() \
if((FLAGS & STEP_MOTOR_DIR_MASK) == STEP_MOTOR_DIR_FORWARD)\
  {\
    HAL_GPIO_WritePin(motor->DIR_GPIO_Port,motor->DIR_GPIO_Pin, GPIO_PIN_SET);\
  }\
  else\
  {\
    HAL_GPIO_WritePin(motor->DIR_GPIO_Port,motor->DIR_GPIO_Pin, GPIO_PIN_RESET);\
  }\
\

void StepMotor_RunStep(StepMotor *motor, int32_t target, int32_t microStepPerSecond,uint16_t attr)
{
#define FLAGS (motor->flags)
#define TARGET (motor->Target_Step)
#define CHECK_STEP (motor->Check_Setp)
#define STEP (motor->Step)
#define SPEED (motor->Target_MicroStepPerSencond)
  FLAGS = 0;
  STEP = 0;
  CHECK_STEP = 1;
  motor->acce = 30000;
  motor->pre_f = 0;
  SPEED = microStepPerSecond;
  FLAGS |= (STEP_MOTOR_MODE_POSITION | attr);
  
  // STEP_MOTOR_DIR_CONTROL_CODE();
  if((FLAGS & STEP_MOTOR_DIR_MASK) == STEP_MOTOR_DIR_FORWARD)
  {
    HAL_GPIO_WritePin(motor->DIR_GPIO_Port,motor->DIR_GPIO_Pin, GPIO_PIN_SET);
  }
  else
  {
    HAL_GPIO_WritePin(motor->DIR_GPIO_Port,motor->DIR_GPIO_Pin, GPIO_PIN_RESET);\
  }


  TARGET = target;
  MotorRun(motor);

#undef FLAGS
#undef TARGET
#undef CHECK_STEP
#undef STEP
}

void StepMotor_RunSpeed(StepMotor *motor, int32_t microStepPerSecond,uint16_t attr)
{
#define FLAGS (motor->flags)
#define CHECK_STEP (motor->Check_Setp)
#define STEP (motor->Step)
#define SPEED (motor->Target_MicroStepPerSencond)
  FLAGS = 0;
  STEP = 0;
  CHECK_STEP = 1;
  motor->acce = 30000;
  motor->pre_f = 0;
  SPEED = microStepPerSecond;
  FLAGS |= (STEP_MOTOR_MODE_VELOCITY | attr);
  
  STEP_MOTOR_DIR_CONTROL_CODE();

  MotorRun(motor);

#undef FLAGS
#undef CHECK_STEP
#undef STEP
#undef SPEED
}

void StepMotor_SetSpeed(StepMotor* motor, int32_t microStepPerSecond)
{
    motor->Target_MicroStepPerSencond = microStepPerSecond;
    motor->flags &= ~STEP_MOTOR_ARRIVED_TARGET_VECL_TURE;
}

void StepMotor_Stop(StepMotor *motor)
{
#define FLAGS (motor->flags)
    motor->Target_MicroStepPerSencond = 0;
    motor->flags &= ~STEP_MOTOR_ARRIVED_TARGET_VECL_TURE;
    FLAGS &= ~STEP_MOTOR_VELOCITY_CONTROL_MASK;
    FLAGS |= STEP_MOTOR_PREPARE_STOP_TURE;
#undef FLAGS
}

void Step_moter_Init(void)
{
    StepMotor_1.Channel = TIM_CHANNEL_1 ;
    StepMotor_1.Step = 0;
    StepMotor_1.TIM = &htim2 ;
    StepMotor_1.Target_Step = 0 ;
    StepMotor_1.state = STOP ;
    StepMotor_1.TIM_CNT_FREQ = 84000000/280;
    StepMotor_1.flags = 0;
    StepMotor_1.DIR_GPIO_Port = GPIOA;
    StepMotor_1.DIR_GPIO_Pin = GPIO_PIN_7;

    StepMotor_2.Channel = TIM_CHANNEL_1 ;
    StepMotor_2.Step = 0;
    StepMotor_2.TIM = &htim13 ;
    StepMotor_2.Target_Step = 0 ;
    StepMotor_2.state = STOP ;
		StepMotor_2.flags = 0;
    StepMotor_2.TIM_CNT_FREQ = 84000000/280 ;
    StepMotor_2.DIR_GPIO_Port = GPIOA;
    StepMotor_2.DIR_GPIO_Pin = GPIO_PIN_4;

    StepMotor_3.Channel = TIM_CHANNEL_1 ;
    StepMotor_3.Step = 0;
    StepMotor_3.TIM = &htim4 ;
    StepMotor_3.Target_Step = 0 ;
    StepMotor_3.state = STOP ;
		StepMotor_3.flags = 0;
    StepMotor_3.TIM_CNT_FREQ = 84000000/280;
    StepMotor_3.DIR_GPIO_Port = GPIOD;
    StepMotor_3.DIR_GPIO_Pin = GPIO_PIN_13;

    StepMotor_4.Channel = TIM_CHANNEL_1 ;
    StepMotor_4.Step = 0;
    StepMotor_4.TIM = &htim10 ;
    StepMotor_4.Target_Step = 0 ;
    StepMotor_4.state = STOP ;
		StepMotor_4.flags = 0;
    StepMotor_4.TIM_CNT_FREQ = 84000000/280;
    StepMotor_4.DIR_GPIO_Port = GPIOB;
    StepMotor_4.DIR_GPIO_Pin = GPIO_PIN_4;
}

void Step_moter_OnePeriod(  const StepMotor *aStepMotor, uint8_t direct  )
{

//  aStepMotor->TIM->Instance->ARR = 300;
    HAL_TIM_Base_Start_IT( aStepMotor->TIM);
    HAL_TIM_PWM_Start(aStepMotor->TIM, aStepMotor->Channel);
    StepMotor_1.state = RUN ;

  while(StepMotor_1.state == RUN )
  {
  }
}

//1���ָߵ�ƽ ��ת
//2���ָߵ�ƽ ��ת
//3���ָߵ�ƽ ��ת
//4���ָߵ�ƽ ��ת
void Step_Translation(  uint32_t target )
{
    target = Distance_To_Step( target );

    // HAL_GPIO_WritePin( GPIOA ,  GPIO_PIN_7  , GPIO_PIN_RESET );   //1��
    // HAL_GPIO_WritePin( GPIOA ,  GPIO_PIN_4  , GPIO_PIN_RESET );   //2��
    // HAL_GPIO_WritePin( GPIOD ,  GPIO_PIN_13 , GPIO_PIN_RESET );  //3��
    // HAL_GPIO_WritePin( GPIOB ,  GPIO_PIN_4  , GPIO_PIN_SET );    //4��

    StepMotor_RunStep(&StepMotor_1, target , 4000 , STEP_MOTOR_DIR_BACKWARD);
    StepMotor_RunStep(&StepMotor_2, target, 4000 , STEP_MOTOR_DIR_FORWARD);
    StepMotor_RunStep(&StepMotor_3, target, 4000 , STEP_MOTOR_DIR_FORWARD);
    StepMotor_RunStep(&StepMotor_4, target , 4000 , STEP_MOTOR_DIR_FORWARD);
}

void Step_Motor_Forward( uint32_t target)
{
    target = Distance_To_Step( target );
    HAL_GPIO_WritePin( GPIOA , GPIO_PIN_7  , GPIO_PIN_SET );  //��1
    HAL_GPIO_WritePin( GPIOA ,  GPIO_PIN_4 , GPIO_PIN_SET );   //��2
    HAL_GPIO_WritePin( GPIOD ,  GPIO_PIN_13 , GPIO_PIN_SET );   //��3
    HAL_GPIO_WritePin( GPIOB ,  GPIO_PIN_4 , GPIO_PIN_SET );    //��4

    StepMotor_RunStep(&StepMotor_1, target , 4000 , STEP_MOTOR_DIR_FORWARD);
    StepMotor_RunStep(&StepMotor_2, target, 4000 , STEP_MOTOR_DIR_FORWARD);
    StepMotor_RunStep(&StepMotor_3, target, 4000 , STEP_MOTOR_DIR_FORWARD);
    StepMotor_RunStep(&StepMotor_4, target , 4000 , STEP_MOTOR_DIR_FORWARD);


}
void Step_Motor_Backward( uint32_t target)
{
    target = Distance_To_Step( target );
    HAL_GPIO_WritePin( GPIOA , GPIO_PIN_7  , GPIO_PIN_RESET );  //��1
    HAL_GPIO_WritePin( GPIOA ,  GPIO_PIN_4 , GPIO_PIN_RESET );   //��2
    HAL_GPIO_WritePin( GPIOD ,  GPIO_PIN_13 , GPIO_PIN_RESET );   //��3
    HAL_GPIO_WritePin( GPIOB ,  GPIO_PIN_4 , GPIO_PIN_RESET );    //��4

    StepMotor_RunStep(&StepMotor_1, target , 4000 , STEP_MOTOR_DIR_FORWARD);
    StepMotor_RunStep(&StepMotor_2, target, 4000 , STEP_MOTOR_DIR_FORWARD);
    StepMotor_RunStep(&StepMotor_3, target, 4000 , STEP_MOTOR_DIR_FORWARD);
    StepMotor_RunStep(&StepMotor_4, target , 4000 , STEP_MOTOR_DIR_FORWARD);


}

void Step_Turn_Circle( uint32_t target )
{
	while(STEP_MOTOR_FLAGS_MASK(&StepMotor_1,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_2,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_3,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_4,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
	
  target = Distance_To_Step( target );
  StepMotor_RunStep(&StepMotor_1,  target , 4000 , STEP_MOTOR_DIR_BACKWARD);
  StepMotor_RunStep(&StepMotor_2,  target , 4000 , STEP_MOTOR_DIR_FORWARD);
  StepMotor_RunStep(&StepMotor_3,  target , 4000 , STEP_MOTOR_DIR_BACKWARD);
  StepMotor_RunStep(&StepMotor_4,  target , 4000 , STEP_MOTOR_DIR_BACKWARD);
	
	while(STEP_MOTOR_FLAGS_MASK(&StepMotor_1,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_2,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_3,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_4,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
}

void Step_Turn_Circle_right( uint32_t target )
{
	while(STEP_MOTOR_FLAGS_MASK(&StepMotor_1,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_2,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_3,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_4,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
	
  target = Distance_To_Step( target );
  StepMotor_RunStep(&StepMotor_1,  target , 4000 , STEP_MOTOR_DIR_FORWARD);
  StepMotor_RunStep(&StepMotor_2,  target , 4000 , STEP_MOTOR_DIR_BACKWARD);
  StepMotor_RunStep(&StepMotor_3,  target , 4000 , STEP_MOTOR_DIR_FORWARD);
  StepMotor_RunStep(&StepMotor_4,  target , 4000 , STEP_MOTOR_DIR_FORWARD);
	
	while(STEP_MOTOR_FLAGS_MASK(&StepMotor_1,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_2,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_3,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_4,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
}

void CAR_MotionsControl(uint32_t attr, uint32_t target)
{
  target = Distance_To_Step( target );
  uint32_t mask = attr & CAR_MOTION_TYPE_MASK;
  union 
  {
    uint8_t dir_sequ[4];
    uint32_t dir_value;
  }dir;
  
  if(mask != 0)
  {
#define SPEED 4000
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_1,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_2,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_3,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    while(STEP_MOTOR_FLAGS_MASK(&StepMotor_4,STEP_MOTOR_STATE_MASK) != STEP_MOTOR_STATE_STOP);
    switch(mask)
    {
      case CAR_MOTION_TYPE_TRANSLATION_FORWARD: 
        dir.dir_value = (STEP_MOTOR_DIR_FORWARD << 0) | (STEP_MOTOR_DIR_FORWARD << 8) | (STEP_MOTOR_DIR_FORWARD << 16) | (STEP_MOTOR_DIR_BACKWARD << 24);
      break;
      case CAR_MOTION_TYPE_TRANSLATION_BACKWARD:
        dir.dir_value = (STEP_MOTOR_DIR_BACKWARD << 0) | (STEP_MOTOR_DIR_BACKWARD << 8) | (STEP_MOTOR_DIR_BACKWARD << 16) | (STEP_MOTOR_DIR_FORWARD << 24);
      break;
      case CAR_MOTION_TYPE_TRANSLATION_LEFT:
        dir.dir_value = (STEP_MOTOR_DIR_BACKWARD << 0) | (STEP_MOTOR_DIR_FORWARD << 8) | (STEP_MOTOR_DIR_FORWARD << 16) | (STEP_MOTOR_DIR_FORWARD << 24);
      break;
      case CAR_MOTION_TYPE_TRANSLATION_RIGHT:
        dir.dir_value = (STEP_MOTOR_DIR_FORWARD << 0) | (STEP_MOTOR_DIR_BACKWARD << 8) | (STEP_MOTOR_DIR_BACKWARD << 16) | (STEP_MOTOR_DIR_BACKWARD << 24);
      break;
      case CAR_MOTION_TYPE_ROTATION_CLOCKWISE:  break;
      case CAR_MOTION_TYPE_ROTATION_ANITICLOCKWISE: break;
      default:break;
    }
    StepMotor_RunStep(&StepMotor_1, target, SPEED, dir.dir_sequ[0]);
    StepMotor_RunStep(&StepMotor_2, target, SPEED, dir.dir_sequ[1]);
    StepMotor_RunStep(&StepMotor_3, target, SPEED, dir.dir_sequ[2]);
    StepMotor_RunStep(&StepMotor_4, target, SPEED, dir.dir_sequ[3]);
#undef SPEED
  }
}
